#include <stdio.h>
#include <termios.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include "JY61P.h"
#include <stdint.h>


#define JY61P_FRAME_LEN 11

//角度转换函数
static float convert_raw_to_angle(int16_t raw) {
    return (float)raw / 32768.0f * 180.0f;
}


// 函数：
// 描述：从数据缓冲区解析jy61p数据，并填充到结构体(jy61p.h)中
// 参数：
//   - frame: 指向包含jy61p数据的缓冲区的指针
//   - data1: 指向结构体 jy61p_data 的指针，用于存储解析后的jy61p数据
// 返回值：整型
int get_jy61p_data(const uint8_t *frame, struct jy61p_data *data1) {
    

    //似乎这一段代码已经进行了帧头的检查
    if (frame == NULL || data1 == NULL) {
        return -1;  // 参数错误
    }
    if (frame[0] != 0x55 || frame[1] != 0x53) {
        return -1;  // 帧头错误
    }
    
    //校验和
    uint8_t sum1 = 0;
    for (int j = 0; j < 10; ++j) {
        sum1 += frame[j];
    }
    if (sum1 != frame[10]) {
        return -1; // 校验失败
    }
    
    // 解析数据
    int16_t raw_roll  = (int16_t)(frame[2] | (frame[3] << 8));
    int16_t raw_pitch = (int16_t)(frame[4] | (frame[5] << 8));
    int16_t raw_yaw   = (int16_t)(frame[6] | (frame[7] << 8));

    // 将原始数据转换为角度
    data1->roll  = convert_raw_to_angle(raw_roll);
    data1->pitch = convert_raw_to_angle(raw_pitch);
    data1->yaw   = convert_raw_to_angle(raw_yaw);

    return 0;  // 成功
   

}

